
#include "motor.h"
/*
motor::motor()
: m_status(FAST_STOP), m_speed(0)
{
  init();
  set_status(FAST_STOP);
}
*/
void motor::init()
{
  //PA5~PA7
  //DDRA = (DDRA&0b00011111)|0b11100000;
  DDRK = 0xff;
  
  //PP0(L) & PP1(R)(PWM))
  PWME_PWME0 = 1; //Enable
  PWME_PWME1 = 1;
  
  PWMPOL_PPOL0 = 1; //Polarity
  PWMPOL_PPOL1 = 1;
  //select Clock SB
  PWMCLK_PCLK0 = 1;
  PWMCLK_PCLK1 = 1;

  PWMPRCLK = (PWMPRCLK&0x8F)|0x10; //Clock B = bus / 2  <bus = 24MHz>
  PWMCTL_CON01 = 0; //DONOT concatenat channel 0 and 1
  PWMCTL_CON23 = 0;
  PWMSCLB = 0x30; //Clock SB = Clock B / (2 * PWMSCLB) //
  
  PWMPER0 = 0xff;  //	 //f = 1495Hz
  PWMPER1 = 0xff;
  
  PWMDTY0 = 0x00;  //	 //duty = 
  PWMDTY1 = 0x00;
}

void motor::set_status_L(motor_status status)
{
  switch (status) 
  {
    case FORWARD:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY0 = m_speed_L;		//D2
      PORTK_BIT3=1;
      PORTK_BIT4=0;
      //PORTA_BIT5 = 1;       //IN1
      //PORTA_BIT6 = 0;       //IN2 
      break;
    case REVERSE:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY0 = m_speed_L;		//D2
      PORTK_BIT3=0;
      PORTK_BIT4=1;
      //PORTA_BIT5 = 0;       //IN1
      //PORTA_BIT6 = 1;       //IN2      
      break;
    case FREE_RUN:
      //PORTA_BIT7 = 1;			  //D1
      PORTK_BIT3=1;
      PORTK_BIT4=1;
      break;
    case FAST_STOP:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY0 = 255;		    //D2
      PORTK_BIT3=0;
      PORTK_BIT4=0;
      //PORTA_BIT5 = 0;       //IN1
      //PORTA_BIT6 = 0;       //IN2      
      break;
  }
  m_status_L = status;
}

void motor::set_status_R(motor_status status)
{
  switch (status) 
  {
    case FORWARD:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY1 = m_speed_R;		//D2
      PORTK_BIT5=0;
      PORTK_BIT7=1;
      //PORTA_BIT5 = 1;       //IN1
      //PORTA_BIT6 = 0;       //IN2 
      break;
    case REVERSE:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY1 = m_speed_R;		//D2
      PORTK_BIT5=1;
      PORTK_BIT7=0;
      //PORTA_BIT5 = 0;       //IN1
      //PORTA_BIT6 = 1;       //IN2      
      break;
    case FREE_RUN:
      //PORTA_BIT7 = 1;			  //D1
      PORTK_BIT5=1;
      PORTK_BIT7=1;
      break;
    case FAST_STOP:
      //PORTA_BIT7 = 0;			  //D1
      PWMDTY1 = 255;		    //D2
      PORTK_BIT5=0;
      PORTK_BIT7=0;
      //PORTA_BIT5 = 0;       //IN1
      //PORTA_BIT6 = 0;       //IN2      
      break;
  }
  m_status_R = status;
}

void motor::set_speed_L(xuint8 speed) 
{
  m_speed_L = speed;
  PWMDTY0 = m_speed_L;
}

void motor::set_speed_R(xuint8 speed) 
{
  m_speed_R = speed;
  PWMDTY1 = m_speed_R;
}


